//
//  foc_nlp.cpp
//  NLCEQ_mixed
//
//  Created by Duong Ngo on 8/13/16.
//  Copyright © 2016 Duong Ngo. All rights reserved.
//

#include "foc_nlp.hpp"
#include "global.hpp"
#include "mapping.hpp"
#include "Jacobian.hpp"
#include "equations.hpp"

#include <cassert>

/** Constructor */
foc_nlp::foc_nlp(const std::vector<double>& x_init, const std::vector<double>& ss)
{
    x_init_ = x_init;
    ss_ = ss;
}

/** Destructor */
foc_nlp::~foc_nlp()
{}

/** Return the size of problem */
bool foc_nlp::get_nlp_info(Index& n, Index& m, Index& nnz_jac_g,
                              Index& nnz_h_lag, IndexStyleEnum& index_style)
{
    n= (T+1)*var_foc;
    m= n;
    nnz_jac_g= 38928;
    nnz_h_lag= 14;
    index_style= TNLP::C_STYLE;
    return true;
}

/** Return the variables bound */
bool foc_nlp::get_bounds_info(Index n, Number* x_l, Number* x_u,
                                 Index m, Number* g_l, Number* g_u)
{

    for (int i=0; i<n; ++i){
        x_l[i]=-2e19;
        x_u[i]= 2e19;
    }
    
    for (int i=0; i<m; ++i){
        g_l[i]=0;
        g_u[i]=0;
    }

    
    return true;
}

/** Return the starting point */
bool foc_nlp::get_starting_point(Index n, bool init_x, Number* x,
                                    bool init_z, Number* z_L, Number* z_U,
                                    Index m, bool init_lambda,
                                    Number* lambda)
{
    assert(init_x == true);
    for (int i=0; i<n; ++i){
        x[i]= x_init_[i];
    }
    return true;
}

/** Evaluate the objective function */
bool foc_nlp::eval_f(Index n, const Number* x, bool new_x, Number& obj_value)
{
    obj_value = 0;
    return true;
}

/** Evaluate the gradient of the objective function */
bool foc_nlp::eval_grad_f(Index n, const Number* x, bool new_x, Number* grad_f)
{
    for (int i=0; i<n; ++i){
        grad_f[i]=0;
    }
    
    return true;
}

/** Evaluate the residual of the constraints */
bool foc_nlp::eval_g(Index n, const Number* x, bool new_x, Index m, Number* g)
{
    
    summarize_all_equations(ss_, x, g);
/*
    for (int i=0; i<m; ++i){
        printf("g[%2u]= %.5e\n",i, g[i]);
    }
*/
    return true;
}

/* Return the Jacobian of g */
bool foc_nlp::eval_jac_g(Index n, const Number* x, bool new_x,
                            Index m, Index nele_jac, Index* iRow, Index *jCol,
                            Number* values)
{
    int dem=0; //track the position of nonzero element in the Jacobian
    if (values == NULL){
        // retun the structure of the Jacobian
        summarize_jac_struct(iRow, jCol, dem);
        printf("Jac Struct dem=%2u\n",dem);
        assert(dem=nele_jac);
    }
    else {
        //return the values of Jacobian
        summarize_jac_values(x, values, dem);
        //printf("Jac Values dem=%2u\n",dem);
        assert(dem=nele_jac);
    }
    return true;
}

/** Return the Hessian Matrix */
bool foc_nlp::eval_h(Index n, const Number* x, bool new_x,
                        Number obj_factor, Index m, const Number* lambda,
                        bool new_lambda, Index nele_hess, Index* iRow,
                        Index* jCol, Number* values)
{
       return false;
}

/** Finalize the Solution */
void foc_nlp::finalize_solution(SolverReturn status,
                                   Index n, const Number* x, const Number* z_L, const Number* z_U,
                                   Index m, const Number* g, const Number* lambda,
                                   Number obj_value,
                                   const IpoptData* ip_data,
                                   IpoptCalculatedQuantities* ip_cq)
{
    result.resize(n);
    max_error = 0;
    for (int i=0; i<n; ++i){
        result[i]= x[i];
        if (fabs(g[i]) > max_error){
            max_error = fabs(g[i]);
        }
    }
}













